#include "motor.h"

void pwm_motors_init()
{
         /* Timer/Counter Control Registers for Timers 0 and 2 */
         TCCR0A = TCCR0A_INIT;
         TCCR0B = TCCR0B_INIT;
         TCCR2A = TCCR2A_INIT;
         TCCR2B = TCCR2B_INIT;

         /* Set DDR for output on OC pins */
         DDROCA |= DDROCA_MASK;
         DDROCB |= DDROCB_MASK;

         // Set DDR for motor direction pins
         DDR_DIR |= DDR_DIR_MASK;
}

void pwm_set_duty (enum motor_number n, int duty)
{
         switch(n)
         {
         case LEFT_MOTOR:

                 if (duty >= 0)
                 {
                         /* Set direction */
                         PORTC |= _BV(LEFT_DIR);
                         /* Set magnitude */
                         TOC_LEFT = (uint8_t) duty;
                 }
                 else
                 {
                         /* Set direction */
                         PORTC &= ~(_BV(LEFT_DIR));
                         /* Set magnitude */
                         TOC_LEFT = (uint8_t) (0 - duty);
                 }
                 break;
         case CENTER_MOTOR:
                 if (duty >= 0)
                 {
                         /* Set direction */
                         PORTC |= _BV(CENTER_DIR);
                         /* Set magnitude */
                         TOC_CENTER = (uint8_t) duty;
                 }
                 else
                 {
                         /* Set direction */
                         PORTC &= ~(_BV(CENTER_DIR));
                         /* Set magnitude */
                         TOC_CENTER = (uint8_t) (0 - duty);
                 }
                 break;
         case RIGHT_MOTOR:
                 if (duty >= 0)
                 {
                         /* Set direction */
                         PORTC |= _BV(RIGHT_DIR);
                         /* Set magnitude */
                         TOC_RIGHT = (uint8_t) duty;
                 }
                 else
                 {
                         /* Set direction */
                         PORTC &= ~(_BV(RIGHT_DIR));
                         /* Set magnitude */
                         TOC_RIGHT = (uint8_t) (0 - duty);
                 }
                 break;
         }
}
